package org.moka.ioio.ioiopilot.input;

import org.moka.ioio.ioiopilot.IOIOThread;

import ioio.lib.api.exception.ConnectionLostException;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.util.Log;

public class PhoneSensorInputs extends AbstractInput implements SensorEventListener  {

	public static final int ACC_X = 0;
	public static final int ACC_Y = 1;
	public static final int ACC_Z = 2;
	public static final int GYRO_X = 3;
	public static final int GYRO_Y = 4;
	public static final int GYRO_Z = 5;
	public static final int MAG_ROLL = 6;
	public static final int MAG_PITCH = 7;
	public static final int MAG_AZIMUT = 8;
	public static final int LIN_ACC_X = 9;
	public static final int LIN_ACC_Y = 10;
	public static final int LIN_ACC_Z = 11;

	public static final int NUM_CHANNELS = 12;
	
	private SensorManager sensorManager;
	private Sensor sAccelerometer;
	private Sensor sMagnetic;
	private Sensor sLinear;
	private Sensor sGyro;


	private float[] inR = new float[16];
	private float[] accArray = new float[3];
	private float[] accRawArray = new float[3];	
	private float[] accZeroArray = new float[3];
	private float[] magRawArray = new float[3];
	private float[] oriArray = new float[3];
	private float[] oriRawArray = new float[3];
	private float[] oriZeroArray = new float[3];
	private float[] gravityAcc = new float[3];
	private float[] linearAcc = new float[3];
	private float[] linearRawArray = new float[3];
	private float[] accPrev = new float[3];
	private float[] gyroArray = new float[3];
	private float[] gyroRawArray = new float[3];

	private final float pi = (float) Math.PI;
	private final float rad2deg = 180/pi;
	
	private int count = 0;
	private long time;
	private boolean calibrated = false;

	private float alpha;
	private float dX;
	private float dY;
	private float dZ;
	private float dT;
	
	
	
	public PhoneSensorInputs(Context context)  {
		super(NUM_CHANNELS);

		sensorManager=(SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
		sAccelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		sMagnetic = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
		
		sLinear = sensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
		if(sLinear!=null) Log.i(IOIOThread.TAG, sLinear.getName()+" ACTIVE ");
		else Log.w(IOIOThread.TAG, "Sensor.TYPE_LINEAR_ACCELERATION not available");
		
		sGyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
		if(sGyro!=null) Log.i(IOIOThread.TAG, sGyro.getName()+" ACTIVE ");
		else Log.w(IOIOThread.TAG, "Sensor.TYPE_GYROSCOPE not available");
		
        sensorManager.registerListener(this, sAccelerometer, SensorManager.SENSOR_DELAY_GAME);		
        sensorManager.registerListener(this, sMagnetic, SensorManager.SENSOR_DELAY_GAME);
        if(sLinear!=null)sensorManager.registerListener(this, sLinear, SensorManager.SENSOR_DELAY_GAME);
        if(sGyro!=null)sensorManager.registerListener(this, sGyro, SensorManager.SENSOR_DELAY_GAME);

        
	}

	 




	@Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void onSensorChanged(SensorEvent event) {
		switch (event.sensor.getType()){  
		case Sensor.TYPE_ACCELEROMETER:
			accRawArray = event.values.clone();
			
			// IF DEVICE DOESN'T SUPPORT LINEAR ACC DO MANUAL CALCULATION
			if(sLinear==null) calculateLinearAcceleration();
			calculateAcceleration();
			if(sGyro==null) calculateGyro();
						
			break;
		case Sensor.TYPE_MAGNETIC_FIELD:
			magRawArray = event.values.clone();
			calculateOrientation();
			break;
		case Sensor.TYPE_LINEAR_ACCELERATION:
			linearAcc = event.values.clone();
			value[LIN_ACC_X] = (int)(linearAcc[0]*500/9.8);
			value[LIN_ACC_Y] = (int)(linearAcc[1]*500/9.8);
			value[LIN_ACC_Z] = (int)(linearAcc[2]*500/9.8);
			break;
		case Sensor.TYPE_GYROSCOPE:
			gyroArray = event.values.clone();
			value[GYRO_X] = (int) -(gyroArray[1]*100);
			value[GYRO_Y] = (int) (gyroArray[0]*100);
			value[GYRO_Z] = (int) (gyroArray[2]*100);
			break;
		}
	}
	private void calculateAcceleration() {
		/*
		 * CALCOLO ACC
		 * */
		accArray[0] = accRawArray[0]-accZeroArray[0];
		accArray[1] = accRawArray[1]-accZeroArray[1];
		accArray[2] = accRawArray[2]-accZeroArray[2];
		
		if(calibrated)
		{
			value[ACC_X] = (int)(accArray[0]*500/9.8);
			value[ACC_Y] = (int)(accArray[1]*500/9.8);
			value[ACC_Z] = (int)(accArray[2]*500/9.8+500);
		}
	}




	private void calculateLinearAcceleration() {

			// NO LINEAR SENSOR... MANUALLY CALCULATE LIN ACC
			// alpha is calculated as t / (t + dT)
	        // with t, the low-pass filter's time-constant
	        // and dT, the event delivery rate
	
			alpha = 0.8f; // (float) 0.8;			
	        gravityAcc[0] = alpha * gravityAcc[0] + (1 - alpha) * accRawArray[0];
	        gravityAcc[1] = alpha * gravityAcc[1] + (1 - alpha) * accRawArray[1];
	        gravityAcc[2] = alpha * gravityAcc[2] + (1 - alpha) * accRawArray[2];
	
	        linearAcc[0] = accRawArray[0] - gravityAcc[0];
	        linearAcc[1] = accRawArray[1] - gravityAcc[1];
	        linearAcc[2] = accRawArray[2] - gravityAcc[2];
		
		if(calibrated)
		{
	        value[LIN_ACC_X] = (int)(linearAcc[0]*500/9.8);
			value[LIN_ACC_Y] = (int)(linearAcc[1]*500/9.8);
			value[LIN_ACC_Z] = (int)(linearAcc[2]*500/9.8);
		}
	}

	public void calculateGyro()
	{
		if(time>0)
		{
			dX = (accArray[0]-linearAcc[0])-accPrev[0];
			dY = (accArray[1]-linearAcc[1])-accPrev[1];
			dZ = (accArray[2]-linearAcc[2])-accPrev[2];
			dT = System.nanoTime() - time;
			gyroArray[0] = (gyroArray[0] + dX*100000000/dT)/2;
			gyroArray[1] = (gyroArray[1] + dY*100000000/dT)/2;
			gyroArray[2] = (gyroArray[2] + dZ*100000000/dT)/2;
			
			value[GYRO_X] = (int) (gyroArray[0]*100);
			value[GYRO_Y] = (int) (gyroArray[1]*100);
			value[GYRO_Z] = (int) (gyroArray[2]*100);
			
			/*Log.d(IOIOThread.TAG,					
					"GYRO X: " + value[GYRO_X] + " V " + 
					"Y: " + value[GYRO_Y] + " V " + 
					"Z: " + value[GYRO_Z] + " V "+
					"dX: "+dX+ " dT: "+dT);
					*/
		}
		time = System.nanoTime();		
		accPrev[0] = accArray[0]-linearAcc[0];
		
	}


	private void calculateOrientation() {
		if (accRawArray != null && magRawArray != null)
		{
			/*float[] accNetRawArray = new float[3];
			accNetRawArray[0] = accRawArray[0]-linearAcc[0];
			accNetRawArray[1] = accRawArray[1]-linearAcc[1];
			accNetRawArray[2] = accRawArray[2]-linearAcc[2];
			*/
			// checks that the rotation matrix is found
			boolean success = SensorManager.getRotationMatrix(inR, null, accRawArray, magRawArray);
			if (success){

				// Finds the Azimuth and Pitch angles of the y-axis with  
				// magnetic north and the horizon respectively
				SensorManager.getOrientation(inR, oriRawArray);
				
				
				if(!calibrated && count>10)calibrate();
				
				oriArray[0] = oriRawArray[0]-oriZeroArray[0];
				if(oriArray[0]<-Math.PI)oriArray[0] = (oriArray[0] + 2*pi);
				else if(oriArray[0]>Math.PI)oriArray[0] = (oriArray[0] - 2*pi);
				
				oriArray[1] = oriRawArray[1]-oriZeroArray[1];
				if(oriArray[1]<-Math.PI)oriArray[1] = (oriArray[1] + 2*pi);
				else if(oriArray[1]>Math.PI)oriArray[1] = (oriArray[1] - 2*pi);
				
				oriArray[2] = oriRawArray[2]-oriZeroArray[2];
				if(oriArray[2]<-Math.PI)oriArray[2] = (oriArray[2] + 2*pi);
				else if(oriArray[2]>Math.PI)oriArray[2] = (oriArray[2] - 2*pi);
				
				if(calibrated)
				{
					value[MAG_ROLL] = (int)(-(oriArray[2]*1000)/pi);
					value[MAG_PITCH] = (int)(-(oriArray[1]*1000)/pi);
					value[MAG_AZIMUT] = (int)((oriArray[0]*500)/pi);
				}
				
				if(count>100)
				{
					
					Log.v(IOIOThread.TAG,					
						"Roll : " + oriArray[2] + " rad "+ 
						"Pitch: " + oriArray[1] + " rad " + 
						"Azimu: " + oriArray[0] + " rad " );
					Log.v(IOIOThread.TAG,					
							"ACC X: " + accArray[0] + "  m/s^2 " + 
							"ACC Y: " + accArray[1] + "  m/s^2 " + 
							"ACC Z: " + accArray[2] + "  m/s^2 ");
					Log.v(IOIOThread.TAG,					
							"LIN_ACC X: " + linearAcc[0] + " m/s^2 " + 
							"LIN_ACC Y: " + linearAcc[1] + " m/s^2 " + 
							"LIN_ACC Z: " + linearAcc[2] + " m/s^2 " +
							"ALFA: "+alpha);
					Log.v(IOIOThread.TAG,					
							"GYRO X: " + gyroArray[0] + "  m/s^2 " + 
							"GYRO Y: " + gyroArray[1] + "  m/s^2 " + 
							"GYRO Z: " + gyroArray[2] + "  m/s^2 ");
					Log.d(IOIOThread.TAG,					
							"VAL Roll : " + value[MAG_ROLL] + " V "+ 
							"Pitch: " + value[MAG_PITCH] + " V " + 
							"Azimu: " + value[MAG_AZIMUT] + " V " );
					Log.d(IOIOThread.TAG,					
								"VAL ACC X: " + value[ACC_X] + " V " + 
								"ACC Y: " + value[ACC_Y] + " V " + 
								"ACC Z: " + value[ACC_Z] + " V ");
					Log.d(IOIOThread.TAG,					
							"VAL LIN_ACC X: " + value[LIN_ACC_X] + " V "+ 
							"LIN_ACC Y: " + value[LIN_ACC_Y] + " V " + 
							"LIN_ACC Z: " + value[LIN_ACC_Z] + " V " );
					Log.d(IOIOThread.TAG,					
							"VAL GYRO X: " + value[GYRO_X] + " V " + 
							"GYRO Y: " + value[GYRO_Y] + " V " + 
							"GYRO Z: " + value[GYRO_Z] + " V ");
					count=0;
					
				}
				count++;
			} 
		}
	}

	private void calibrate() {
		accZeroArray = accRawArray.clone();
		oriZeroArray = oriRawArray.clone();
		Log.d(IOIOThread.TAG, "Sensors CALIBRATED");
		calibrated = true;
	}
}
